/*!
  @file motion_solver_p3p.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _MOTION_SOLVER_P3P_H_
#define _MOTION_SOLVER_P3P_H_

#include <boost/scoped_ptr.hpp>
#include <opencv2/core/core.hpp>

#include "mrover_vo/feature.h"
#include "mrover_vo/motion_solver.h"

namespace vo {

/*! 
 *  @brief  P3P solver using Gao's algorithm
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  OpenCV implementation
 */

class MotionSolverP3P : public MotionSolver {
 public:
  typedef boost::scoped_ptr<MotionSolverP3P> Ptr;

  using MotionSolver::compute;


  MotionSolverP3P() 
      : use_guess_(false)
      , use_lm_(false)
  { }

  ~MotionSolverP3P() { }

  inline void computeLM(cv::Mat &pose) {
    use_lm_ = true;
    compute(pose);
  }


  virtual void compute();

  void enableInitialGuess() { use_guess_ = true; }
  void disableInitialGuess() { use_guess_ = false; } 


 private:
  virtual inline bool initCompute() {
    if (!MotionSolver::initCompute()) return false;
    return true;
  }

  virtual size_t getRequiredSampleSize() { return 4; }

  //! flag for using initial guess
  bool use_guess_;

  //! internal flag for using iterative algorithm (Levenberg-Marquardt)
  bool use_lm_;

  std::vector<cv::Point3f> obj_pts_;
  std::vector<cv::Point2f> img_pts_;
};

}  // namespace vo

#endif  // _MOTION_SOLVER_P3P_H_
